import matplotlib.pyplot as plt
import numpy as np
from scipy.interpolate import BSpline
  
def B(x, k, i, t):
   if k == 0:
      return 1.0 if t[i] <= x < t[i+1] else 0.0
   if t[i+k] == t[i]:
      c1 = 0.0
   else:
      c1 = (x - t[i])/(t[i+k] - t[i]) * B(x, k-1, i, t)
   if t[i+k+1] == t[i+1]:
      c2 = 0.0
   else:
      c2 = (t[i+k+1] - x)/(t[i+k+1] - t[i+1]) * B(x, k-1, i+1, t)
   return c1 + c2

  
def bspline(x, t, c, k):
   n = len(c)
   return sum(c[i] * B(x, k, i, t) for i in range(n))
   
def main():
   k = 3    #degree, k越大，曲线越逼近原始控制点
   t = []    #knots vector
   c1 = [-1.0, 1.0, 2.0, 2.5, 3.0, 3.0, 3.0, 3.0, 3.5, 5.0, 6.0, 8.0, 9.0] #x轴的路点
   c2 = [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 3.0, 3.0, 3.0,  3.5, 5.0] #y轴的路点
   num = len(c1)
   # print "num:", num, "k:", k
   for i in range(num+k+1):
       if i <= k:
          t.append(0)
       elif i >= num:
          t.append(num-k)
       else:
          t.append(i-k)
   # print "t:", t
   
   spl_x = BSpline(t, c1, k)
   spl_y = BSpline(t, c2, k)
   plt.plot(c1, c2, '-og', label="Waypoints")
   
   xx = np.linspace(0.0, num-k, 100)
   
   plt.plot(spl_x(xx), spl_y(xx), '-r', label="B-Spline")
   plt.grid(True)
   plt.legend()
   plt.axis("equal")
   plt.show()
   
   
if __name__ == '__main__':
   main()